/*
 * TinyEKF: Extended Kalman Filter for Arduino and TeensyBoard.
 *
 * Copyright (C) 2015 Simon D. Levy
 *
 * MIT License
 */

#ifndef FOLLOW_DOWN_TINYEKF_H
#define FOLLOW_DOWN_TINYEKF_H


#include <stdio.h>
#include <stdlib.h>
#include "tiny_ekf_struct.h"
#include "tiny_ekf.h"

/**
 * A header-only class for the Extended Kalman Filter.  Your implementing class should #define the constant N and
 * and then #include <TinyEKF.h>  You will also need to implement a model() method for your application.
 */
class TinyEKF {

private:

    ekf_t ekf;

protected:

    /**
      * The current state.
      */
    double *x;

    /**
     * Initializes a TinyEKF object.
     */
    TinyEKF() {
        ekf_init(&this->ekf, Nsta, Mobs);
        this->x = this->ekf.x;
    }

    /**
     * Deallocates memory for a TinyEKF object.
     */
    ~TinyEKF() {}

    /**
     * Implement this function for your EKF model.
     * @param fx gets output of state-transition function <i>f(x<sub>0 .. n-1</sub>)</i>
     * @param F gets <i>n &times; n</i> Jacobian of <i>f(x)</i>
     * @param hx gets output of observation function <i>h(x<sub>0 .. n-1</sub>)</i>
     * @param H gets <i>m &times; n</i> Jacobian of <i>h(x)</i>
     */
    virtual void model(double fx[Nsta], double F[Nsta][Nsta], double hx[Mobs], double H[Mobs][Nsta]) = 0;

    /**
     * Sets the specified value of the prediction error covariance. <i>P<sub>i,j</sub> = value</i>
     * @param i row index
     * @param j column index
     * @param value value to set
     */
    void setP(int i, int j, double value) {
        this->ekf.P[i][j] = value;
    }

    /**
     * Sets the specified value of the process noise covariance. <i>Q<sub>i,j</sub> = value</i>
     * @param i row index
     * @param j column index
     * @param value value to set
     */
    void setQ(int i, int j, double value) {
        this->ekf.Q[i][j] = value;
    }

    /**
     * Sets the specified value of the observation noise covariance. <i>R<sub>i,j</sub> = value</i>
     * @param i row index
     * @param j column index
     * @param value value to set
     */
    void setR(int i, int j, double value) {
        this->ekf.R[i][j] = value;
    }

public:

    /**
     * Returns the state element at a given index.
     * @param i the index (at least 0 and less than <i>n</i>
     * @return state value at index
     */
    double getX(int i) {
        return this->ekf.x[i];
    }

    /**
     * Sets the state element at a given index.
     * @param i the index (at least 0 and less than <i>n</i>
     * @param value value to set
     */
    void setX(int i, double value) {
        this->ekf.x[i] = value;
    }

    /**
      Performs one step of the prediction and update.
     * @param z observation vector, length <i>m</i>
     * @return true on success, false on failure caused by non-positive-definite matrix.
     */
    bool step(double *z) {
        this->model(this->ekf.fx, this->ekf.F, this->ekf.hx, this->ekf.H);

        return ekf_step(&this->ekf, z) ? false : true;
    }
};


#endif //FOLLOW_DOWN_TINYEKF_H
